import math

import numpy as np

from coordinate_transform import latlon_to_local, local_to_latlon

import math


def calculate_boundary_points(path_points, width):
    """
    根据农机作业路径点计算左边界点和右边界点数组。

    参数:
    path_points: list of list, 每个元素为 [经度, 纬度, 朝向角(弧度)]
    width: float, 农机作业宽度（米）

    返回:
    left_points: list of list, 左边界点数组 [[经度, 纬度], ...]
    right_points: list of list, 右边界点数组 [[经度, 纬度], ...]
    """
    left_points = []
    right_points = []

    for point in path_points:
        lon = point[0]
        lat = point[1]

        angle = np.degrees(point[2])

        lat_rad = math.radians(lat)  # 纬度转换为弧度

        # 计算左边界移动向量（米）
        east_left = -math.cos(angle) * width / 2
        north_left = math.sin(angle) * width / 2

        # 计算右边界移动向量（米）
        east_right = math.cos(angle) * width / 2
        north_right = -math.sin(angle) * width / 2

        # 计算经纬度变化量（度）
        delta_lon_left = east_left / (111000 * math.cos(lat_rad))
        delta_lat_left = north_left / 111000
        delta_lon_right = east_right / (111000 * math.cos(lat_rad))
        delta_lat_right = north_right / 111000

        # 计算左右边界点
        left_point = [lon + delta_lon_left, lat + delta_lat_left]
        right_point = [lon + delta_lon_right, lat + delta_lat_right]

        left_points.append(left_point)
        right_points.append(right_point)

    return left_points, right_points


# 示例使用
if __name__ == '__main__':
    # 示例路径点：假设有三个点，每个点 [经度, 纬度, 朝向角(弧度)]
    path_points = [
        [116.0, 39.0, 0],  # 北朝向
        [116.001, 39.001, math.pi / 2],  # 东朝向
        [116.002, 39.002, math.pi]  # 南朝向
    ]
    width = 10.0  # 作业宽度10米
    left_points, right_points = calculate_boundary_points(path_points, width)
    print("左边界点数组:", left_points)
    print("右边界点数组:", right_points)

def compute_boundaries(path_points, ref_lat, ref_lon, TRACTOR_WIDTH, ax0):

    CAR_WIDTH = TRACTOR_WIDTH

    left_boundary_points = []
    right_boundary_points = []

    for i in range(path_points.shape[0]):

        lon, lat, theta = path_points[i]

        x, y = latlon_to_local(lat, lon, ref_lat, ref_lon)  # 当前路径点在局部坐标系下的坐标
        heading_deg = np.degrees(theta)  # 农机朝向，将弧度转换为角度

        if 0 < heading_deg < 90:
            left_origin_lon, left_origin_lat = local_to_latlon(x - np.sin(theta) * CAR_WIDTH / 2,
                                                               y + np.cos(theta) * CAR_WIDTH / 2, ref_lat,
                                                               ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x + np.sin(theta) * CAR_WIDTH / 2,
                                                                 y - np.cos(theta) * CAR_WIDTH / 2, ref_lat,
                                                                 ref_lon)
        elif 90 < heading_deg < 180:
            left_origin_lon, left_origin_lat = local_to_latlon(x - np.cos(theta - np.pi/2) * CAR_WIDTH / 2,
                                                               y - np.sin(theta - np.pi/2) * CAR_WIDTH / 2, ref_lat,
                                                               ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x + np.cos(theta - np.pi/2) * CAR_WIDTH / 2,
                                                                 y + np.sin(theta - np.pi/2) * CAR_WIDTH / 2, ref_lat,
                                                                 ref_lon)
        elif -180 < heading_deg < -90:
            left_origin_lon, left_origin_lat = local_to_latlon(x + np.cos(-(theta + np.pi/2)) * CAR_WIDTH / 2,
                                                               y - np.sin(-(theta + np.pi/2)) * CAR_WIDTH / 2, ref_lat,
                                                               ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x - np.cos(-(theta + np.pi/2)) * CAR_WIDTH / 2,
                                                                 y + np.sin(-(theta + np.pi/2)) * CAR_WIDTH / 2,
                                                                 ref_lat, ref_lon)

            #temp1 = np.square( (x + np.cos(-(heading_deg + 90)) * CAR_WIDTH / 2) - (x - np.cos(-(heading_deg + 90)) * CAR_WIDTH / 2) ) + np.square( (y - np.sin(-(heading_deg + 90)) * CAR_WIDTH / 2) - (y + np.sin(-(heading_deg + 90)) * CAR_WIDTH / 2) )
            #temp2 = np.sqrt(temp1)

        elif -90 < heading_deg < 0:
            left_origin_lon, left_origin_lat = local_to_latlon(x + np.sin(-theta) * CAR_WIDTH / 2,
                                                               y + np.cos(-theta) * CAR_WIDTH / 2, ref_lat,
                                                               ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x - np.sin(-theta) * CAR_WIDTH / 2,
                                                                 y - np.cos(-theta) * CAR_WIDTH / 2, ref_lat,
                                                                 ref_lon)
        elif heading_deg == 0:
            left_origin_lon, left_origin_lat = local_to_latlon(x, y + CAR_WIDTH / 2, ref_lat, ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x, y - CAR_WIDTH / 2, ref_lat,  ref_lon)
        elif heading_deg == 90:
            left_origin_lon, left_origin_lat = local_to_latlon(x - CAR_WIDTH / 2, y, ref_lat,  ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x + CAR_WIDTH / 2, y, ref_lat,  ref_lon)
        elif heading_deg == 180:
            left_origin_lon, left_origin_lat = local_to_latlon(x - CAR_WIDTH / 2, y , ref_lat, ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x + CAR_WIDTH / 2, y , ref_lat, ref_lon)
        elif heading_deg == -90:
            left_origin_lon, left_origin_lat = local_to_latlon(x + CAR_WIDTH / 2,  y , ref_lat,   ref_lon)
            right_origin_lon, right_origin_lat = local_to_latlon(x - CAR_WIDTH / 2,  y , ref_lat,   ref_lon)



        left_boundary_points.append((left_origin_lon, left_origin_lat))
        right_boundary_points.append((right_origin_lon, right_origin_lat))

        #ax0.scatter(left_origin_lon, left_origin_lat, color='red', edgecolor='black', s=100, zorder=6)

        #ax0.scatter(right_origin_lon, right_origin_lat, color='green', edgecolor='black', s=100, zorder=6)



    return left_boundary_points, right_boundary_points

